#include "hybrid_a_star.h"

HybridAStar::HybridAStar() {}

HybridAStar::~HybridAStar() {}

void HybridAStar::InitParam( const Eigen::Vector3d &start_, const Eigen::Vector3d &goal_, int motion_type_,
                             const nav_msgs::msg::OccupancyGrid::Ptr image_map_,
                             std::vector< std::vector< bool > >     &map_data_ ) {
    RCLCPP_INFO( rclcpp::get_logger( "" ), "cellSize [%.3f]", Constants::cellSize );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "iterations [%d]", Constants::iterations );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "bloating [%.3f]", Constants::bloating );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "veh_W [%.3f]", Constants::veh_W );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "veh_Lf [%.3f]", Constants::veh_Lf );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "veh_Lr [%.3f]", Constants::veh_Lr );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "expand_step [%.3f]", Constants::expand_step );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "move_delta [%.3f]", Constants::move_delta );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "r [%.3f]", Constants::r );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "dx [%.3f,%.3f,%.3f]", Constants::dx[ 0 ], Constants::dx[ 1 ],
                 Constants::dx[ 2 ] );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "dy [%.3f,%.3f,%.3f]", Constants::dy[ 0 ], Constants::dy[ 1 ],
                 Constants::dy[ 2 ] );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "dt [%.3f,%.3f,%.3f]", Constants::dt[ 0 ], Constants::dt[ 1 ],
                 Constants::dt[ 2 ] );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "penaltyTurning [%.3f]", Constants::penaltyTurning );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "penaltyReversing [%.3f]", Constants::penaltyReversing );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "penaltySteerChange [%.3f]", Constants::penaltySteerChange );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "dubinsShotDistance [%.3f]", Constants::dubinsShotDistance );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "dubinsStepSize [%.3f]", Constants::dubinsStepSize );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "move_step [%.3f]", Constants::move_step );

    image_map = image_map_;
    // 相对于像素坐标系的坐标
    start.head( 2 ) =
        ( start_.head( 2 ) - Eigen::Vector2d( image_map->info.origin.position.x, image_map->info.origin.position.y ) )
        / Constants::cellSize;
    goal.head( 2 ) =
        ( goal_.head( 2 ) - Eigen::Vector2d( image_map->info.origin.position.x, image_map->info.origin.position.y ) )
        / Constants::cellSize;
    start.z()   = start_.z();
    goal.z()    = goal_.z();
    motion_type = motion_type_;

    map_width  = image_map->info.width;
    map_height = image_map->info.height;

    map_data.resize( map_width * map_height );
    for ( int h = 0; h < map_height; ++h ) {
        for ( int w = 0; w < map_width; ++w ) {
            map_data[ h * map_width + w ] = map_data_[ w ][ h ];
        }
    }

    RCLCPP_INFO( rclcpp::get_logger( "planner.cpp" ), "map_data set done!" );

    expand_node_publisher_ = ros_node->create_publisher< geometry_msgs::msg::PoseArray >( "expand_node", 20 );
    dubin_path_publisher_  = ros_node->create_publisher< nav_msgs::msg::Path >( "dubin_path", 10 );
    dubin_veh_shape_visual_publisher_ =
        ros_node->create_publisher< visualization_msgs::msg::Marker >( "dubin_veh_shape", 10 );
}

void HybridAStar::showMapData() {
    // RCLCPP_INFO( rclcpp::get_logger( "" ), "map_width: [%d]", map_width );
    // RCLCPP_INFO( rclcpp::get_logger( "" ), "map_height: [%d]", map_height );
    // for ( const auto d : map_data ) {
    //     RCLCPP_INFO( rclcpp::get_logger( "" ), "Map_data_00: [%d]", ( int )d );
    // }
}

bool HybridAStar::plan() {
    int length_2d = map_width * map_height;
    int length_3d = length_2d * Constants::headings;

    // auto                            time_start_1 = std::chrono::high_resolution_clock::now();
    // Node3D                         *nodes3D      = new Node3D[ length_3d ]();
    // auto                            time_end_1   = std::chrono::high_resolution_clock::now();
    // std::chrono::duration< double > diff_1       = ( time_end_1 - time_start_1 ) * 1000;
    // RCLCPP_INFO( rclcpp::get_logger( "" ), "vector< Node3D > cost time: %.5f ms", diff_1.count() );

    Node3D nGoal( goal.x(), goal.y(), Tools::normalize_angle( goal.z() ), 0, 0, nullptr );
    Node3D nStart( start.x(), start.y(), Tools::normalize_angle( start.z() ), 0, 0, nullptr );

    RCLCPP_INFO( rclcpp::get_logger( "" ), "nStart: [%.3f,%.3f,%.3f]", nStart.getX(), nStart.getY(), nStart.getT() );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "nGoal: [%.3f,%.3f,%.3f]", nGoal.getX(), nGoal.getY(), nGoal.getT() );

    auto    time_start = std::chrono::high_resolution_clock::now();
    Node3D *nSolution  = hybridAStarSearch( nStart, nGoal );
    // Node3D                         *nSolution = hybridAStarSearch( nStart, nGoal, nodes3D );
    auto                            time_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration< double > diff     = ( time_end - time_start ) * 1000;
    RCLCPP_INFO( rclcpp::get_logger( "" ), "hybridAStarSearch function cost time: %.5f ms", diff.count() );

    tracePath( nSolution );  // 终止节点回溯得到起点到终点的节点集合
    RCLCPP_INFO( rclcpp::get_logger( "" ), "node_path.Size: [%d]", node_path.size() );

    // delete[] nodes3D;

    return !node_path.empty();
}

Node3D *HybridAStar::hybridAStarSearch( Node3D &start_node, Node3D &goal_node ) {
    int dir  = 3;
    int iter = 1;

    priorityQueue                                         openList;
    std::unordered_map< uint64_t, Node3D * >              openMap;
    std::unordered_set< uint64_t, std::hash< uint64_t > > closedIndex;

    updateH( start_node, goal_node );
    openList.push( &start_node );
    openMap.insert( { start_node.setIdx( map_width, map_height ), &start_node } );

    if ( Constants::debug )
        showExpandNode( goal_node, iter );

    Node3D *current_node = nullptr, *expand_node = nullptr;

    RCLCPP_INFO( rclcpp::get_logger( "" ), "Start hybrid a star search loop" );
    while ( !openList.empty() ) {
        current_node = openList.top();
        openList.pop();

        if ( Constants::debug )
            showExpandNode( *current_node, iter );

        int curr_idx = current_node->setIdx( map_width, map_height );
        if ( closedIndex.find( curr_idx ) != closedIndex.end() )
            continue;

        openMap.erase( curr_idx );
        closedIndex.insert( curr_idx );

        if ( current_node->isArrivedGoal( goal_node ) ) {
            RCLCPP_INFO( rclcpp::get_logger( "" ), "Expand node is arrived goal node" );
            return current_node;
        } else if ( current_node->isInDubinRange( goal_node ) ) {
            if ( Constants::debug )
                showDubinPath( *current_node, goal_node );
            expand_node = dubinsShot( *current_node, goal_node );
            if ( expand_node != nullptr ) {
                RCLCPP_INFO( rclcpp::get_logger( "" ), "Dubin curver shot successfully, iteration %d", iter );
                if ( expand_node->isTheSameNode( goal_node ) ) {
                    return expand_node;
                } else {
                    goal_node.setPred( expand_node );
                    return &goal_node;
                }
            }
        }

        for ( int i = 0; i < dir; i++ ) {
            expand_node = current_node->createExpandNode( i, motion_type );
            if ( Constants::debug )
                showExpandNode( *expand_node, iter );

            if ( checkCollision( expand_node ) ) {
                int expand_idx = expand_node->setIdx( map_width, map_height );
                if ( closedIndex.find( expand_idx ) == closedIndex.end() || expand_idx == curr_idx ) {
                    expand_node->updateG();
                    auto node_it = openMap.find( expand_idx );
                    if ( node_it == openMap.end() || expand_node->getG() < node_it->second->getG()
                         || expand_idx == curr_idx ) {
                        updateH( *expand_node, goal_node );

                        if ( expand_idx == curr_idx
                             && expand_node->getC() > current_node->getC() + Constants::tieBreaker ) {
                            delete expand_node;
                            continue;
                        } else if ( expand_idx == curr_idx
                                    && expand_node->getC() <= current_node->getC() + Constants::tieBreaker ) {
                            expand_node->setPred( current_node->getPred() );
                        }

                        if ( expand_node->getPred() == expand_node ) {
                            RCLCPP_INFO( rclcpp::get_logger( "" ), "looping" );
                        }

                        openMap.insert( { expand_idx, expand_node } );
                        openList.push( expand_node );
                    } else {
                        delete expand_node;
                    }
                } else {
                    delete expand_node;
                }
            } else {
                delete expand_node;
            }
        }

        iter++;
        if ( iter > Constants::iterations ) {
            RCLCPP_INFO( rclcpp::get_logger( "" ), "Current iteration is over the max_iter" );
            return nullptr;
        }
    }
    RCLCPP_INFO( rclcpp::get_logger( "" ), "OpenList is empty, replan failed! iteration: %d", iter );
    return nullptr;
}

Node3D *HybridAStar::hybridAStarSearch( Node3D &start, const Node3D &goal, Node3D *nodes3D ) {
    int           iPred, iSucc;
    float         newG;
    int           dir        = 3 + 2 * Constants::dir_discrete_num;
    int           iterations = 0;
    priorityQueue O;

    updateH( start, goal );
    start.open();
    O.push( &start );
    iPred            = start.setIdx( map_width, map_height );
    nodes3D[ iPred ] = start;
    if ( Constants::debug )
        showExpandNode( goal, iterations );

    Node3D *nPred;
    Node3D *nSucc;

    RCLCPP_INFO( rclcpp::get_logger( "" ), "HA* loop" );
    while ( !O.empty() ) {
        nPred = O.top();
        iPred = nPred->setIdx( map_width, map_height );
        iterations++;
        if ( Constants::debug )
            showExpandNode( *nPred, iterations );

        if ( iterations > Constants::iterations ) {
            RCLCPP_INFO( rclcpp::get_logger( "" ), "Current iteration is over the max iter" );
            return nullptr;
        }

        if ( nodes3D[ iPred ].isClosed() ) {
            O.pop();
            continue;
        } else if ( nodes3D[ iPred ].isOpen() ) {
            nodes3D[ iPred ].close();
            O.pop();

            if ( nPred->isArrivedGoal( goal ) ) {
                RCLCPP_INFO( rclcpp::get_logger( "" ), "*nPred == goal, iteration %d", iterations );
                return nPred;
            } else {
                if ( nPred->isInDubinRange( goal ) ) {
                    if ( Constants::debug )
                        showDubinPath( *nPred, goal );
                    nSucc  = dubinsShot( *nPred, goal );
                    lambda = 1.0;
                    if ( nSucc != nullptr ) {
                        RCLCPP_INFO( rclcpp::get_logger( "" ), "Dubin curver shot, iteration %d", iterations );
                        return nSucc;
                    }
                }
                for ( int i = 0; i < dir; i++ ) {
                    nSucc = nPred->createExpandNode( i, motion_type );
                    iSucc = nSucc->setIdx( map_width, map_height );
                    if ( Constants::debug )
                        showExpandNode( *nSucc, iterations );

                    if ( checkCollision( nSucc ) ) {
                        if ( !nodes3D[ iSucc ].isClosed() || iPred == iSucc ) {
                            nSucc->updateG();
                            newG = nSucc->getG();

                            if ( !nodes3D[ iSucc ].isOpen() || newG < nodes3D[ iSucc ].getG() || iPred == iSucc ) {
                                updateH( *nSucc, goal );

                                if ( iPred == iSucc && nSucc->getC() > nPred->getC() + Constants::tieBreaker ) {
                                    delete nSucc;
                                    continue;
                                } else if ( iPred == iSucc && nSucc->getC() <= nPred->getC() + Constants::tieBreaker ) {
                                    nSucc->setPred( nPred->getPred() );
                                }

                                if ( nSucc->getPred() == nSucc ) {
                                    RCLCPP_INFO( rclcpp::get_logger( "" ), "looping" );
                                }

                                nSucc->open();
                                nodes3D[ iSucc ] = *nSucc;
                                O.push( &nodes3D[ iSucc ] );
                                delete nSucc;
                            } else {
                                delete nSucc;
                            }
                        } else {
                            delete nSucc;
                        }
                    } else {
                        delete nSucc;
                    }
                }
            }
        }
    }

    if ( O.empty() ) {
        RCLCPP_INFO( rclcpp::get_logger( "" ), "O.empty(), iteration: %d", iterations );
        return nullptr;
    }
    return nullptr;
}

void HybridAStar::updateH( Node3D &start, const Node3D &goal ) {
    float DubinsCost    = 0;
    float EularDistCost = 0;

    ompl::base::DubinsStateSpace dubinsPath( Constants::r );
    State                       *dbStart   = ( State                         *)dubinsPath.allocState();
    State                       *dbEnd     = ( State                           *)dubinsPath.allocState();
    double                       start_yaw = start.getT();
    double                       goal_yaw  = goal.getT();
    if ( motion_type < 0 ) {
        start_yaw = Tools::normalize_angle( start_yaw + M_PI );
        goal_yaw  = Tools::normalize_angle( goal_yaw + M_PI );
    }
    dbStart->setXY( start.getX(), start.getY() );
    dbStart->setYaw( start_yaw );
    dbEnd->setXY( goal.getX(), goal.getY() );
    dbEnd->setYaw( goal_yaw );
    DubinsCost = dubinsPath.distance( dbStart, dbEnd );

    EularDistCost = sqrt( pow( ( start.getX() - goal.getX() ), 2 ) + pow( ( start.getY() - goal.getY() ), 2 ) );

    start.setH( lambda * std::max( DubinsCost, EularDistCost ) );
}

bool HybridAStar::checkCollision( const Node3D *node ) {
    if ( !node->isOnGrid( map_width, map_height ) )
        return false;

    Eigen::Vector2d curr_state( node->getX(), node->getY() );
    Eigen::Vector2d point_0, point_1, point_2, point_3;
    Eigen::Matrix2d Rt;
    Rt << cos( node->getT() ), -sin( node->getT() ), sin( node->getT() ), cos( node->getT() );
    point_0 = curr_state + Rt * Eigen::Vector2d( -Constants::veh_Lr, Constants::veh_W / 2 ) / Constants::cellSize;
    point_1 = curr_state + Rt * Eigen::Vector2d( Constants::veh_Lf, Constants::veh_W / 2 ) / Constants::cellSize;
    point_2 = curr_state + Rt * Eigen::Vector2d( Constants::veh_Lf, -Constants::veh_W / 2 ) / Constants::cellSize;
    point_3 = curr_state + Rt * Eigen::Vector2d( -Constants::veh_Lr, -Constants::veh_W / 2 ) / Constants::cellSize;

    if ( !LineCheck( point_0, point_1 ) ) {
        return false;
    }

    if ( !LineCheck( point_1, point_2 ) ) {
        return false;
    }

    if ( !LineCheck( point_2, point_3 ) ) {
        return false;
    }

    if ( !LineCheck( point_3, point_0 ) ) {
        return false;
    }

    return true;
}

bool HybridAStar::LineCheck( const Eigen::Vector2d &p1, const Eigen::Vector2d &p2 ) {
    double          dist   = ( p2 - p1 ).norm();
    int             num    = ( int )dist;
    Eigen::Vector2d curr_p = p1;
    for ( int i = 0; i < num; ++i ) {
        curr_p += ( p2 - p1 ) / dist;
        double px    = curr_p.x();
        double py    = curr_p.y();
        int    index = ( int )curr_p.y() * map_width + ( int )curr_p.x();
        if ( map_data[ index ] ) {
            return false;
        }
    }
    return true;
}

Node3D *HybridAStar::dubinsShot( Node3D &start, const Node3D &goal ) {
    double q0[] = { start.getX(), start.getY(), start.getT() };
    double q1[] = { goal.getX(), goal.getY(), goal.getT() };
    if ( motion_type < 0 )  // 针对倒车时Dubin曲线连接起点终点
    {
        q0[ 2 ] = Tools::normalize_angle( q0[ 2 ] + M_PI );
        q1[ 2 ] = Tools::normalize_angle( q1[ 2 ] + M_PI );
    }
    DubinsPath path;
    dubins_init( q0, q1, Constants::r, &path );

    int   i      = 0;
    float x      = 0.f;
    float length = dubins_path_length( &path );

    Node3D *dubinsNodes = new Node3D[ ( int )( length / Constants::dubinsStepSize ) + 1 ];

    while ( x < length ) {
        double q[ 3 ];
        dubins_path_sample( &path, x, q );
        dubinsNodes[ i ].setX( q[ 0 ] );
        dubinsNodes[ i ].setY( q[ 1 ] );
        if ( motion_type < 0 )
            q[ 2 ] = Tools::normalize_angle( q[ 2 ] + M_PI );
        dubinsNodes[ i ].setT( Tools::normalize_angle( q[ 2 ] ) );
        if ( Constants::debug )
            showVehShape( dubinsNodes[ i ] );

        if ( checkCollision( &dubinsNodes[ i ] ) ) {
            if ( i > 1 ) {
                dubinsNodes[ i ].setPred( &dubinsNodes[ i - 1 ] );
            } else if ( i == 1 ) {
                dubinsNodes[ i ].setPred( &start );
            }

            if ( &dubinsNodes[ i ] == dubinsNodes[ i ].getPred() ) {
                std::cout << "looping shot";
            }

            x += Constants::dubinsStepSize;
            i++;
        } else {
            delete[] dubinsNodes;
            return nullptr;
        }
    }
    return &dubinsNodes[ i - 1 ];
}

void HybridAStar::tracePath( const Node3D *node, int i, std::vector< Node3D > path ) {
    if ( node == nullptr ) {
        this->node_path = path;
        std::reverse( node_path.begin(), node_path.end() );
        return;
    }

    i++;
    path.push_back( *node );
    tracePath( node->getPred(), i, path );
}

void HybridAStar::calcInterPoint( const Eigen::Vector3d &start_node, int dir_type,
                                  std::vector< Eigen::Vector4d > &path ) {
    int    num       = ( int )( Constants::expand_step / Constants::move_step );
    float  rest_dist = Constants::expand_step - num * Constants::move_step;
    float  delta_yaw = Constants::move_delta * ( 1 - rest_dist / Constants::expand_step );
    double x         = start_node.x();
    double y         = start_node.y();
    double yaw       = start_node.z();
    double reverse_yaw;
    if ( motion_type < 0 ) {
        reverse_yaw = Tools::normalize_angle( yaw + M_PI );
    } else {
        reverse_yaw = yaw;
    }
    path.push_back( Eigen::Vector4d( x * Constants::cellSize + image_map->info.origin.position.x,
                                     y * Constants::cellSize + image_map->info.origin.position.y, reverse_yaw,
                                     motion_type ) );
    for ( int i = 0; i < num - 1; i++ ) {
        x += motion_type * Constants::move_step * cos( yaw );
        y += motion_type * Constants::move_step * sin( yaw );
        switch ( dir_type ) {
        case 0:
            break;
        case 1:
            yaw += motion_type * delta_yaw / num;
            break;
        case 2:
            yaw -= motion_type * delta_yaw / num;
        default:
            break;
        }
        if ( motion_type < 0 ) {
            reverse_yaw = Tools::normalize_angle( yaw + M_PI );
        } else {
            reverse_yaw = Tools::normalize_angle( yaw );
        }
        path.push_back( Eigen::Vector4d( x * Constants::cellSize + image_map->info.origin.position.x,
                                         y * Constants::cellSize + image_map->info.origin.position.y, reverse_yaw,
                                         motion_type ) );
    }
}

// std::vector< Eigen::Vector4d > HybridAStar::returnPath() {
//     std::vector< Eigen::Vector4d > path;
//     for ( int i = 0; i < node_path.size() - 1; i++ ) {
//         Eigen::Vector3d start_node( node_path[ i ].getX(), node_path[ i ].getY(), node_path[ i ].getT() );
//         Eigen::Vector3d goal_node( node_path[ i + 1 ].getX(), node_path[ i + 1 ].getY(), node_path[ i + 1 ].getT() );

//         double node_dist =
//             sqrt( pow( ( start_node.x() - goal_node.x() ), 2 ) + pow( ( start_node.y() - goal_node.y() ), 2 ) );
//         if ( node_dist > 1.5 * Constants::dubinsStepSize )  // 此时非杜宾曲线节点
//         {
//             int dir_type = node_path[ i + 1 ].getPrim();
//             calcInterPoint( start_node, dir_type, path );
//         } else {
//             Eigen::Vector4d pp;
//             pp.x() = node_path[ i ].getX() * Constants::cellSize + image_map->info.origin.position.x;
//             pp.y() = node_path[ i ].getY() * Constants::cellSize + image_map->info.origin.position.y;
//             // double reverse_yaw;
//             // if(motion_type < 0)
//             // {
//             // 	reverse_yaw = Tools::normalize_angle(node_path[i].getT() + M_PI);
//             // }
//             // else
//             // {
//             // 	reverse_yaw = node_path[i].getT();
//             // }
//             // pp.z() = reverse_yaw;
//             pp.z() = node_path[ i ].getT();
//             pp.w() = motion_type;
//             path.push_back( pp );
//         }
//     }
//     Eigen::Vector4d pp;
//     pp.x() = node_path.back().getX() * Constants::cellSize + image_map->info.origin.position.x;
//     pp.y() = node_path.back().getY() * Constants::cellSize + image_map->info.origin.position.y;
//     double reverse_yaw;
//     if ( motion_type < 0 ) {
//         reverse_yaw = Tools::normalize_angle( node_path.back().getT() + M_PI );
//     } else {
//         reverse_yaw = node_path.back().getT();
//     }
//     pp.z() = reverse_yaw;
//     pp.w() = motion_type;
//     path.push_back( pp );

//     return path;
// }

std::vector< Eigen::Vector4d > HybridAStar::returnPath() {
    std::vector< Eigen::Vector4d > path;
    for ( int i = 0; i < node_path.size(); i++ ) {
        Eigen::Vector4d pp;
        pp.x() = node_path[ i ].getX() * Constants::cellSize + image_map->info.origin.position.x;
        pp.y() = node_path[ i ].getY() * Constants::cellSize + image_map->info.origin.position.y;
        double reverse_yaw;
        if ( motion_type < 0 ) {
            reverse_yaw = Tools::normalize_angle( node_path[ i ].getT() + M_PI );
        } else {
            reverse_yaw = node_path[ i ].getT();
        }
        pp.z() = reverse_yaw;
        pp.w() = motion_type;
        path.push_back( pp );
        if ( Constants::debug )
            showVehShape( node_path[ i ] );
    }

    return path;
}

void HybridAStar::showExpandNode( const Node3D &node, int iterations ) {
    geometry_msgs::msg::PoseArray current_search_tree;
    current_search_tree.header.frame_id = "map";
    current_search_tree.header.stamp    = ros_node->get_clock()->now();

    geometry_msgs::msg::Pose pose;
    pose.position.x = node.getX() * Constants::cellSize + image_map->info.origin.position.x;
    pose.position.y = node.getY() * Constants::cellSize + image_map->info.origin.position.y;
    pose.position.z = 0;
    tf2::Quaternion quat;
    quat.setRPY( 0, 0, node.getT() );  // 使用欧拉角设置绕 z 轴旋转45度
    pose.orientation.x = quat.x();
    pose.orientation.y = quat.y();
    pose.orientation.z = quat.z();
    pose.orientation.w = quat.w();

    vec_expand_points.emplace_back( pose );

    std::string iter = std::to_string( iterations );
    RCLCPP_INFO( rclcpp::get_logger( iter ), "showExpandNode [%.3f,%.3f,%.3f]", node.getX(), node.getY(), node.getT() );

    for ( const auto &p : vec_expand_points ) {
        current_search_tree.poses.emplace_back( p );
    }

    expand_node_publisher_->publish( current_search_tree );

    showVehShape( node );
}

void HybridAStar::showDubinPath( Node3D &start, const Node3D &goal ) {
    double q0[] = { start.getX(), start.getY(), start.getT() };
    double q1[] = { goal.getX(), goal.getY(), goal.getT() };
    if ( motion_type < 0 ) {
        q0[ 2 ] = Tools::normalize_angle( q0[ 2 ] + M_PI );
        q1[ 2 ] = Tools::normalize_angle( q1[ 2 ] + M_PI );
    }
    DubinsPath path;
    dubins_init( q0, q1, Constants::r, &path );

    int   i      = 0;
    float x      = 0.f;
    float length = dubins_path_length( &path );

    Node3D *dubinsNodes = new Node3D[ ( int )( length / Constants::dubinsStepSize ) + 1 ];

    while ( x < length ) {
        double q[ 3 ];
        dubins_path_sample( &path, x, q );
        dubinsNodes[ i ].setX( q[ 0 ] );
        dubinsNodes[ i ].setY( q[ 1 ] );
        dubinsNodes[ i ].setT( Tools::normalize_angle( q[ 2 ] ) );

        if ( i > 0 ) {
            dubinsNodes[ i ].setPred( &dubinsNodes[ i - 1 ] );
        } else {
            dubinsNodes[ i ].setPred( &start );
        }

        if ( &dubinsNodes[ i ] == dubinsNodes[ i ].getPred() ) {
            std::cout << "looping shot";
        }

        x += Constants::dubinsStepSize;
        i++;
    }
    dubinsNodes[ 1 ].setPred( &start );

    nav_msgs::msg::Path dubin_path;
    dubin_path.header.frame_id = "map";
    for ( int k = i - 1; k >= 0; k-- ) {
        geometry_msgs::msg::PoseStamped p;
        Node3D                          node = dubinsNodes[ k ];
        p.pose.position.x                    = node.getX() * Constants::cellSize + image_map->info.origin.position.x;
        p.pose.position.y                    = node.getY() * Constants::cellSize + image_map->info.origin.position.y;
        dubin_path.poses.emplace_back( p );
    }

    dubin_path_publisher_->publish( dubin_path );
}

void HybridAStar::showVehShape( const Node3D &node ) {
    visualization_msgs::msg::Marker veh_marker;

    veh_marker.header.frame_id = "map";
    veh_marker.header.stamp    = ros_node->get_clock()->now();
    veh_marker.ns              = "dubin_veh";
    veh_marker.id              = 0;

    veh_marker.type   = visualization_msgs::msg::Marker::CUBE;
    veh_marker.action = visualization_msgs::msg::Marker::ADD;

    double x   = node.getX() * Constants::cellSize + image_map->info.origin.position.x;
    double y   = node.getY() * Constants::cellSize + image_map->info.origin.position.y;
    double yaw = node.getT();

    veh_marker.pose.position.x = x + Constants::car_length_ / 2 * cos( yaw );
    veh_marker.pose.position.y = y + Constants::car_length_ / 2 * sin( yaw );
    veh_marker.pose.position.z = 0;

    tf2::Quaternion quat;
    quat.setRPY( 0, 0, node.getT() );  // 使用欧拉角设置绕 z 轴旋转45度
    veh_marker.pose.orientation.x = quat.x();
    veh_marker.pose.orientation.y = quat.y();
    veh_marker.pose.orientation.z = quat.z();
    veh_marker.pose.orientation.w = quat.w();

    veh_marker.scale.x = Constants::veh_Lf + Constants::veh_Lr;
    veh_marker.scale.y = Constants::veh_W;
    veh_marker.scale.z = 0.01;

    veh_marker.color.r = 0.4f;
    veh_marker.color.g = 0.2f;
    veh_marker.color.b = 0.3f;
    veh_marker.color.a = 0.5;

    dubin_veh_shape_visual_publisher_->publish( veh_marker );
}
